%%% Gait detection data analysis %%%
close all;
clear all;
clc;

%% loading files

filename = 'orthosis_static1';
% load .anc file
[labels_anc,time_anc,EMG,rel_angle,step_number,forces] = readColDataANC(strcat(filename,'.anc'),23,11,1);

% load .trc file
[labels,time_trc,LASIS,RASIS,Sacrum,Sac_offset,LThigh_up,LThigh_front,LThigh_rear,LKnee_lat,LKnee_med,...
    LShank_up, LShank_front, LShank_rear, LAnkle_lat,LAnkle_med, LHeel, LToe, RThigh_up,RThigh_front,RThigh_rear,RKnee_lat,RKnee_med,...
    RShank_up, RShank_front, RShank_rear, RAnkle_lat,RAnkle_med, RHeel, RToe, RTrochanter, LTrochanter]...
    = readColDataMA(strcat(filename,'.trc'),92,6,1);
% [time_trc,Rtoe,Rankle,Rheel,Ltoe,Lankle,Lheel] = readColDataXLS_MA(filename);

%% analyzing data
dt_trc = time_trc(2)-time_trc(1);
dt_anc = time_anc(2)-time_anc(1);
Lforce = forces(:,13:16)./204.8;
Lforce_totalz = sum(Lforce,2);
Rforce = forces(:,5:8)./204.8;
Rforce_totalz = sum(Rforce,2);

Lforce_x = forces(:,9:10)./204.8;
Lforce_totalx = sum(Lforce_x,2);
Lforce_y = forces(:,11:12)./204.8;
Lforce_totaly = sum(Lforce_y,2);
Lforce_mag = sqrt(Lforce_totalx.^2 + Lforce_totaly.^2 + Lforce_totalz.^2);
Lforce_dir = 100*[Lforce_totalx./Lforce_mag Lforce_totaly./Lforce_mag Lforce_totalz./Lforce_mag];

Rforce_x = forces(:,1:2)./204.8;
Rforce_totalx = sum(Lforce_x,2);
Rforce_y = forces(:,3:4)./204.8;
Rforce_totaly = sum(Lforce_y,2);
Rforce_mag = sqrt(Rforce_totalx.^2 + Rforce_totaly.^2 + Rforce_totalz.^2);
Rforce_dir = 100*[Rforce_totalx./Rforce_mag Rforce_totaly./Rforce_mag Rforce_totalz./Rforce_mag];

plate_length = 600; %mm
plate_width = 300; %mm
plate_offset = 300; %mm
CoP_L = [(Lforce(:,1)+Lforce(:,2))./Lforce_totalz * plate_length  (Lforce(:,1)+Lforce(:,4))./Lforce_totalz * plate_width + plate_offset];
CoP_R = [(Rforce(:,1)+Rforce(:,2))./Rforce_totalz * plate_length  (Rforce(:,1)+Rforce(:,4))./Rforce_totalz * plate_width];

%x-direction is perpendicular to treadmill
max_ydata = [RAnkle_lat(:,1) LAnkle_lat(:,1) RToe(:,1) LToe(:,1)]/1000;
max_xdata = [RToe(:,2) LToe(:,2) RHeel(:,2) LHeel(:,2)]/1000;
max_zdata = [zeros(length(Sacrum),1) Sacrum(:,3) Sac_offset(:,3)]/1000;

%
% LASIS,RASIS,Sacrum,Sac_offset,LThigh_up,LThigh_front,LThigh_rear,LKnee_lat,LKnee_med,...
%     LShank_up, LShank_front, LShank_rear, LAnkle_lat,LAnkle_med, LHeel, LToe, RThigh_up,RThigh_front,RThigh_rear,RKnee_lat,RKnee_med,...
%     RShank_up, RShank_front, RShank_rear, RAnkle_lat,RAnkle_med, RHeel, RToe, RTrochanter, LTrochanter]
xdata = [LASIS(:,1),RASIS(:,1),Sacrum(:,1),LThigh_up(:,1),LThigh_front(:,1),LThigh_rear(:,1),...
    LShank_up(:,1), LShank_front(:,1), LShank_rear(:,1),  LAnkle_lat(:,1), LHeel(:,1), LToe(:,1), RThigh_up(:,1),RThigh_front(:,1),RThigh_rear(:,1),...
    RShank_up(:,1), RShank_front(:,1), RShank_rear(:,1),RAnkle_lat(:,1),RHeel(:,1), RToe(:,1), RTrochanter(:,1), LTrochanter(:,1),Sac_offset(:,1), ...
    LKnee_lat(:,1),LKnee_med(:,1), LAnkle_med(:,1), RKnee_lat(:,1),RKnee_med(:,1),RAnkle_med(:,1)]/1000;

ydata = [LASIS(:,2),RASIS(:,2),Sacrum(:,2),LThigh_up(:,2),LThigh_front(:,2),LThigh_rear(:,2),...
    LShank_up(:,2), LShank_front(:,2), LShank_rear(:,2), LAnkle_lat(:,2), LHeel(:,2), LToe(:,2), RThigh_up(:,2),RThigh_front(:,2),RThigh_rear(:,2),...
    RShank_up(:,2), RShank_front(:,2), RShank_rear(:,2), RAnkle_lat(:,2),RHeel(:,2), RToe(:,2), RTrochanter(:,2), LTrochanter(:,2),Sac_offset(:,2), ...
    LKnee_lat(:,2), LKnee_med(:,2), LAnkle_med(:,2), RKnee_lat(:,2), RKnee_med(:,2), RAnkle_med(:,2)]/1000;

zdata = [LASIS(:,3),RASIS(:,3),Sacrum(:,3),LThigh_up(:,3),LThigh_front(:,3),LThigh_rear(:,3),...
    LShank_up(:,3), LShank_front(:,3), LShank_rear(:,3), LAnkle_lat(:,3), LHeel(:,3), LToe(:,3), RThigh_up(:,3),RThigh_front(:,3),RThigh_rear(:,3),...
    RShank_up(:,3), RShank_front(:,3), RShank_rear(:,3), RAnkle_lat(:,3), RHeel(:,3), RToe(:,3), RTrochanter(:,3), LTrochanter(:,3),Sac_offset(:,3),...
    LKnee_lat(:,3), LKnee_med(:,3), LAnkle_med(:,3), RKnee_lat(:,3), RKnee_med(:,3), RAnkle_med(:,3)]/1000;

% Rankle_angle = atan2(RToe(:,3)-RHeel(:,3),RToe(:,1)-RHeel(:,1));
% thigh_angle = atan2(zdata(:,14)-zdata(:,16),xdata(:,14)-xdata(:,16))*180/pi; 
% shank_angle = atan2(zdata(:,17)-zdata(:,19),xdata(:,17)-xdata(:,19))*180/pi;
% knee_angle = thigh_angle-shank_angle;
x = [1 0 0]; y = [0 1 0]; z = [0 0 1];


%% Pelvis Coords
[Pelvis P_x P_y P_z] = coord_def([xdata(1,3) ydata(1,3) zdata(1,3)], [xdata(1,1) ydata(1,1) zdata(1,1)], [xdata(1,2) ydata(1,2) zdata(1,2)]);
RHip_p_CoR = (inv(Pelvis)*[xdata(1,22) ydata(1,22) zdata(1,22) 1]')';
LHip_p_CoR = (inv(Pelvis)*[xdata(1,23) ydata(1,23) zdata(1,23) 1]')';
%% Right Thigh Coords
[RThigh_rot RThigh_x RThigh_y RThigh_z] = coord_def([xdata(1,13) ydata(1,13) zdata(1,13)], [xdata(1,14) ydata(1,14) zdata(1,14)], [xdata(1,15) ydata(1,15) zdata(1,15)]);
RKnee_CoR = (inv(RThigh_rot)*[xdata(1,28) ydata(1,28) zdata(1,28),1]')';  % finding knee center relative to thigh
RKnee_CoR_med = (inv(RThigh_rot)*[xdata(1,29) ydata(1,29) zdata(1,29),1]')';  %finding medial knee center rel to thigh
RHip_CoR = (inv(RThigh_rot)*[xdata(1,22) ydata(1,22) zdata(1,22),1]')';   % finding hip relative to thigh

%% Right Shank Coords
[RShank_rot RShank_x RShank_y RShank_z] = coord_def([xdata(1,16) ydata(1,16) zdata(1,16)], [xdata(1,17) ydata(1,17) zdata(1,17)], [xdata(1,18) ydata(1,18) zdata(1,18)]);
RKnee_CoR_shank = (inv(RShank_rot)*[xdata(1,28) ydata(1,28) zdata(1,28),1]')';
RKnee_CoR_med_shank = (inv(RShank_rot)*[xdata(1,29) ydata(1,29) zdata(1,29),1]')';

%% Right Medial Ankle Coords
[Rank_rot Rank_x Rank_y Rank_z] = coord_def([xdata(1,19) ydata(1,19) zdata(1,19)], [xdata(1,20) ydata(1,20) zdata(1,20)], [xdata(1,21) ydata(1,21) zdata(1,21)]);
Rank_CoR = (inv(Rank_rot)*[xdata(1,30) ydata(1,30) zdata(1,30),1]')';

%% Left Thigh Coords
[LThigh_rot LThigh_x LThigh_y LThigh_z] = coord_def([xdata(1,4) ydata(1,4) zdata(1,4)], [xdata(1,5) ydata(1,5) zdata(1,5)], [xdata(1,6) ydata(1,6) zdata(1,6)]);          
LKnee_CoR = (inv(LThigh_rot)*[xdata(1,25) ydata(1,25) zdata(1,25) 1]')';
LKnee_CoR_med = (inv(LThigh_rot)*[xdata(1,26) ydata(1,26) zdata(1,26) 1]')';
LHip_CoR = (inv(LThigh_rot)*[xdata(1,23) ydata(1,23) zdata(1,23),1]')';

%% Calculating Left shank coords
[LShank_rot LShank_x LShank_y LShank_z] = coord_def([xdata(1,7) ydata(1,7) zdata(1,7)], [xdata(1,8) ydata(1,8) zdata(1,8)], [xdata(1,9) ydata(1,9) zdata(1,9)]);
LKnee_CoR_shank = (inv(LShank_rot)*[xdata(1,25) ydata(1,25) zdata(1,25) 1]')';
LKnee_CoR_med_shank = (inv(LShank_rot)*[xdata(1,26) ydata(1,26) zdata(1,26) 1]')';

%% Left Medial Ankle Coords
[Lank_rot Lank_x Lank_y Lank_z] = coord_def([xdata(1,10) ydata(1,10) zdata(1,10)], [xdata(1,11) ydata(1,11) zdata(1,11)], [xdata(1,12) ydata(1,12) zdata(1,12)]);
Lank_CoR = (inv(Lank_rot)*[xdata(1,27) ydata(1,27) zdata(1,27),1]')';


%% Plotting
fh = figure(1);
% subplot(2,1,1)
hold on;
axis([min(min(max_xdata)) max(max(max_xdata)) min(min(max_ydata)) max(max(max_ydata)) min(min(max_zdata)) max(max(max_zdata))]);
view(45,45);

PELVIS = patch(xdata(1,1:3),ydata(1,1:3),zdata(1,1:3),[.5 .5 .5]);
LEFT_THIGH = patch(xdata(1,4:6),ydata(1,4:6),zdata(1,4:6),[.3 .3 .3]);
LEFT_SHANK = patch(xdata(1,7:9),ydata(1,7:9),zdata(1,7:9),[.3 .3 .3]);
LEFT_FOOT = patch(xdata(1,10:12),ydata(1,10:12),zdata(1,10:12),[.3 .3 .3]);
RIGHT_THIGH = patch(xdata(1,13:15),ydata(1,13:15),zdata(1,13:15),[.3 .3 .3]);
RIGHT_SHANK = patch(xdata(1,16:18),ydata(1,16:18),zdata(1,16:18),[.3 .3 .3]);
RIGHT_FOOT = patch(xdata(1,19:21),ydata(1,19:21),zdata(1,19:21),[.3 .3 .3]);
p2 = [xdata(1,13) ydata(1,13) zdata(1,13)];
line([p2(1) p2(1)+RThigh_x(1)], [p2(2) p2(2)+RThigh_x(2)], [p2(3) p2(3)+RThigh_x(3)],'Color','r')
line([p2(1) p2(1)+RThigh_y(1)], [p2(2) p2(2)+RThigh_y(2)], [p2(3) p2(3)+RThigh_y(3)],'Color','g')
line([p2(1) p2(1)+RThigh_z(1)], [p2(2) p2(2)+RThigh_z(2)], [p2(3) p2(3)+RThigh_z(3)],'Color','k')

p2 = [xdata(1,7) ydata(1,7) zdata(1,7)];
line([p2(1) p2(1)+LShank_x(1)], [p2(2) p2(2)+LShank_x(2)], [p2(3) p2(3)+LShank_x(3)],'Color','r')
line([p2(1) p2(1)+LShank_y(1)], [p2(2) p2(2)+LShank_y(2)], [p2(3) p2(3)+LShank_y(3)],'Color','g')
line([p2(1) p2(1)+LShank_z(1)], [p2(2) p2(2)+LShank_z(2)], [p2(3) p2(3)+LShank_z(3)],'Color','k')

p2 = [xdata(1,3) ydata(1,3) zdata(1,3)];
line([p2(1) p2(1)+P_x(1)], [p2(2) p2(2)+P_x(2)], [p2(3) p2(3)+P_x(3)],'Color','r')
line([p2(1) p2(1)+P_y(1)], [p2(2) p2(2)+P_y(2)], [p2(3) p2(3)+P_y(3)],'Color','g')
line([p2(1) p2(1)+P_z(1)], [p2(2) p2(2)+P_z(2)], [p2(3) p2(3)+P_z(3)],'Color','k')

% FORCE_VECTOR_L = line([CoP_L(1,1) CoP_L(1,1)+Lforce_dir(1,1)],[CoP_L(1,2) CoP_L(1,2)+Lforce_dir(1,2)], ...
%     [0 Lforce_dir(1,3)],'Color','k','LineWidth',3);
% FORCE_VECTOR_R = line([CoP_R(1,1) CoP_R(1,1)+Rforce_dir(1,1)],[CoP_R(1,2) CoP_R(1,2)+Rforce_dir(1,2)], ...
%     [0 Rforce_dir(1,3)],'Color','k','LineWidth',3);

for i = 1%1:length(time_trc)
%     subplot(2,1,2)
    set(PELVIS,'xdata',xdata(i,1:3), 'ydata',ydata(i,1:3),'zdata',zdata(i,1:3))
    set(LEFT_THIGH,'xdata',xdata(i,4:6), 'ydata',ydata(i,4:6),'zdata',zdata(i,4:6))
    set(LEFT_SHANK,'xdata',xdata(i,7:9), 'ydata',ydata(i,7:9),'zdata',zdata(i,7:9))
    set(LEFT_FOOT,'xdata',xdata(i,10:12), 'ydata',ydata(i,10:12),'zdata',zdata(i,10:12))
    set(RIGHT_THIGH,'xdata',xdata(i,13:15), 'ydata',ydata(i,13:15),'zdata',zdata(i,13:15))
    set(RIGHT_SHANK,'xdata',xdata(i,16:18), 'ydata',ydata(i,16:18),'zdata',zdata(i,16:18))
    set(RIGHT_FOOT,'xdata',xdata(i,19:21), 'ydata',ydata(i,19:21),'zdata',zdata(i,19:21))
    

    %% angular offsets for ankle, knee and hip
    rshank = atan2(RKnee_lat(i,3) - RAnkle_lat(i,3), RKnee_lat(i,1) - RAnkle_lat(i,1));
    rthigh = atan2(RTrochanter(i,3) - RKnee_lat(i,3), RTrochanter(i,1) - RKnee_lat(i,1));
    rpelvis = atan2(RASIS(i,3) - RTrochanter(i,3), RASIS(i,1) - RTrochanter(i,1));
    rfoot = atan2(RAnkle_lat(i,3) - RToe(i,3), RAnkle_lat(i,1) - RToe(i,1));
    
    rshankf = atan2(RKnee_lat(i,3) - RAnkle_lat(i,3), RKnee_lat(i,2) - RAnkle_lat(i,2));
    rthighf = atan2(RTrochanter(i,3) - RKnee_lat(i,3), RTrochanter(i,2) - RKnee_lat(i,2));
    rpelvisf = atan2(RASIS(i,3) - RTrochanter(i,3), RASIS(i,2) - RTrochanter(i,2));
    rfootf = atan2(RAnkle_lat(i,3) - RToe(i,3), RAnkle_lat(i,2) - RToe(i,2));
    
    lshank = atan2(LKnee_lat(i,3) - LAnkle_lat(i,3), LKnee_lat(i,1) - LAnkle_lat(i,1));
    lthigh = atan2(LTrochanter(i,3) - LKnee_lat(i,3), LTrochanter(i,1) - LKnee_lat(i,1));
    lpelvis = atan2(LASIS(i,3) - LTrochanter(i,3), LASIS(i,1) - LTrochanter(i,1));
    lfoot = atan2(LAnkle_lat(i,3) - LToe(i,3), LAnkle_lat(i,1) - LToe(i,1));
    
    lshankf = atan2(LKnee_lat(i,3) - LAnkle_lat(i,3), LKnee_lat(i,2) - LAnkle_lat(i,2));
    lthighf = atan2(LTrochanter(i,3) - LKnee_lat(i,3), LTrochanter(i,2) - LKnee_lat(i,2));
    lpelvisf = atan2(LASIS(i,3) - LTrochanter(i,3), LASIS(i,2) - LTrochanter(i,2));
    lfootf = atan2(LAnkle_lat(i,3) - LToe(i,3), LAnkle_lat(i,2) - LToe(i,2));
    

    %     set(FORCE_VECTOR_L,'xdata',[CoP_L(10*i,1) CoP_L(10*i,1)+Lforce_mag(10*i)*Lforce_dir(10*i,1)], ...
    %         'ydata',[CoP_L(10*i,2) CoP_L(10*i,2)+Lforce_mag(10*i)*Lforce_dir(10*i,2)],...
    %         'zdata',[0 Lforce_mag(10*i)*Lforce_dir(10*i,3)]);
    %     set(FORCE_VECTOR_R,'xdata',[CoP_R(10*i,1) CoP_R(10*i,1)+Rforce_mag(10*i)*Rforce_dir(10*i,1)], ...
    %         'ydata',[CoP_R(10*i,2) CoP_R(10*i,2)+Rforce_mag(10*i)*Rforce_dir(10*i,2)],...
    %         'zdata',[0 Rforce_mag(10*i)*Rforce_dir(10*i,3)]);

    drawnow
    %     subplot(2,1,2)
    %     hold on;
    %     plot(time_trc(i),Rankle_angle(i),'.r')
    %     pause(dt_trc);

        

end

angle_offsets.pelvis.left.sagittal = lpelvis;
angle_offsets.pelvis.right.sagittal = rpelvis;
angle_offsets.pelvis.left.frontal = lpelvisf;
angle_offsets.pelvis.right.frontal = rpelvisf;

angle_offsets.thigh.left.sagittal = lthigh;
angle_offsets.thigh.right.sagittal = rthigh;
angle_offsets.thigh.left.frontal = lthighf;
angle_offsets.thigh.right.frontal = rthighf;

angle_offsets.shank.left.sagittal = lshank;
angle_offsets.shank.right.sagittal = rshank;
angle_offsets.shank.left.frontal = lshankf;
angle_offsets.shank.right.frontal = rshankf;

angle_offsets.foot.left.sagittal = lfoot;
angle_offsets.foot.right.sagittal = rfoot;
angle_offsets.foot.left.frontal = lfootf;
angle_offsets.foot.right.frontal = rfootf;
save jamie_angle_offsets.mat angle_offsets

axis equal

CoR_matrix = [LHip_CoR; RHip_CoR; LHip_p_CoR; RHip_p_CoR; LKnee_CoR; RKnee_CoR; LKnee_CoR_med; RKnee_CoR_med; ...
    LKnee_CoR_shank; RKnee_CoR_shank; LKnee_CoR_med_shank; RKnee_CoR_med_shank; Lank_CoR; Rank_CoR];

